//
// Created by hyj on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>  
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

struct Pose
{
    Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {};
    Eigen::Matrix3d Rwc;      //旋转矩阵
    Eigen::Quaterniond qwc;   //四元数表示的旋转+平移
    Eigen::Vector3d twc;      //平移向量
};
int main()
{
    int featureNums = 20;     //特征点数量，landmark
    int poseNums = 10;        //位姿数量，相机位姿是六维的，缺少尺度
    int diem = poseNums * 6 + featureNums * 3;  
    double fx = 1.;
    double fy = 1.;
    Eigen::MatrixXd H(diem,diem);  //生成器 120 x 120 的矩阵
    H.setZero(); 
    std::cout << H << std::endl;

    std::vector<Pose> camera_pose;  //定义相机的pose
    double radius = 8;

    /************************************相机位姿——camera做圆弧运动*********************************/
    for(int n = 0; n < poseNums; ++n ) {
        double theta = n * 2 * M_PI / ( poseNums * 4); // 1/4 圆弧
        // 绕 z轴 旋转
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());   //每次绕Z轴旋转 PI/2
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        camera_pose.push_back(Pose(R,t));
    }

    // 随机数生成三维特征点
    std::default_random_engine generator;
    std::vector<Eigen::Vector3d> points;
    for(int j = 0; j < featureNums; ++j)
    {
        std::uniform_real_distribution<double> xy_rand(-4, 4.0);  //创建取值范围
        std::uniform_real_distribution<double> z_rand(8., 10.);
        double tx = xy_rand(generator);
        double ty = xy_rand(generator);
        double tz = z_rand(generator);

        //世界坐标系的特征点
        Eigen::Vector3d Pw(tx, ty, tz);
        //产生世界坐标系中的特征点
        points.push_back(Pw);

        for (int i = 0; i < poseNums; ++i) {
            Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();  //旋转矩阵，世界坐标系——>相机坐标系
            Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);  //得到相机坐标系中的特征点

            double x = Pc.x();
            double y = Pc.y();
            double z = Pc.z();
            double z_2 = z * z;
            Eigen::Matrix<double,2,3> jacobian_uv_Pc;  //相机投影方程关于相机坐标系下的三维点的导数
            jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
                    0, fy/z, -y * fy/z_2;
            // 残差对于世界坐标系下三维坐标点的导数
            Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;
             //误差相对于李代数的雅克比矩阵2×6与视觉SLAM十四讲P196公式（8.15）一致
            Eigen::Matrix<double,2,6> jacobian_Ti;    //误差关于位姿的导数
            jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
                            -(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;

            /*
                block参数
                \param startRow the first row in the block
                \param startCol the first col in the block
                \param blockRows the number of rows in the block
                \param blockCols the number of cols in the block
            */
            H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;     //位姿点对角线
            /// 请补充完整作业信息矩阵块的计算
            H.block(j*3 + 6*poseNums, j*3+6*poseNums, 3, 3) += jacobian_Pj.transpose() * jacobian_Pj;   //空间点对角线
            H.block(i*6, j*3+6*poseNums, 6, 3) += jacobian_Ti.transpose() * jacobian_Pj;  //非对角线
            H.block(j*3+6*poseNums, i*6, 3, 6) += jacobian_Pj.transpose() * jacobian_Ti;
            
        }
    }

//    std::cout << H << std::endl;
//    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H);
//    std::cout << saes.eigenvalues() <<std::endl;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
    std::cout << svd.singularValues() <<std::endl;  //通过SVD分解得到特征值
  
    return 0;
}
